{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "## Алгоритм Дейкстры для планирования пути по 2D карте"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Импорты"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from robobase.routedijkstra import dijkstraGrid\n",
    "from src.support import startAndEntToMap, routeToMap"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Входные параметры"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "outputs": [],
   "source": [
    "input_map = [[0, 0, 0, 1, 1, 1, 0, 0],\n",
    "             [0, 0, 0, 0, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 0, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 1, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0]]\n",
    "\n",
    "start_coords, dest_coords = [7, 1], [7, 6]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Построение карты с начальной и конечной координатой"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 288x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "map_with_start_end = startAndEntToMap(input_map, start_coords, dest_coords)\n",
    "plt.matshow(map_with_start_end)\n",
    "plt.show()"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Расчет маршрута"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "outputs": [],
   "source": [
    "route = dijkstraGrid(input_map, start_coords, dest_coords)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Построение карты с начальной, конечной координатой,  маршрутом"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 288x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "map_with_all = routeToMap(map_with_start_end, route)\n",
    "plt.matshow(map_with_start_end)\n",
    "plt.show()\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}